#ifndef __PENDANT_H__
#define __PENDANT_H__

#include <vector>
#include <bitset>
#include <string>
#include <boost/cstdint.hpp>
#include <boost/asio.hpp>
#include <boost/asio/serial_port.hpp>
#include <boost/system/system_error.hpp>
#include <boost/array.hpp>
#include <boost/bind.hpp>
#include "Device.h"

using boost::uint8_t;
using boost::uint16_t;

namespace device {
namespace pendant {

const int PACKETSIZE = 5;
const unsigned int BAUDRATE = 38400;
typedef boost::array<uint8_t, PACKETSIZE> Packet; 
const double VOLTAGEMULTIPLIER=1.887e-3; // microvolts
const uint16_t ABOVEALC=63;

static inline int packetNumber(uint8_t framebyte, uint8_t plus=0) {
    return ((framebyte >> 6) + plus) % 4;
}
template<typename Listener>
struct Reader { 
    Listener& listener;	
    Packet packet, oldPacket;
    int syncOffset;
    boost::array<bool, PACKETSIZE> syncCandidates;

    Reader(Listener& listener_) : listener(listener_), syncOffset(-1) { 
        syncCandidates.assign(true);
    }

    bool synchronized() const {
        if(!syncCandidates[0])
            return false;
        for(int ii=1; ii<PACKETSIZE; ++ii) 
            if(syncCandidates[ii])
                return false;
        return true;
    }

    /* 
    * if there is no error, packet contains the last 5 bytes read from the pendant
    */
    void operator()(const boost::system::error_code ec, std::size_t bytes_transferred) {
        boost::asio::detail::throw_error(ec);
        if(syncOffset==-1) { 
            syncOffset=0;
            oldPacket=packet;
        } else if(synchronized() && packetNumber(oldPacket[0],1)==packetNumber(packet[0])){
            syncOffset=0;
            oldPacket=packet;
        } else {
            if(synchronized()) 
                // inform self that we're not any longer
                syncCandidates.assign(true);

            //either we weren't synchronized before or synchronization has just broken
            assert(!synchronized());
            for(int ii=0; ii<PACKETSIZE; ++ii)
                syncCandidates[ii] = syncCandidates[ii] && 
                    packetNumber(oldPacket[ii],1)==packetNumber(packet[ii]);
            int count=0;
            for(int ii=0; ii<PACKETSIZE; ++ii)
                count+=syncCandidates[ii];

            if(count==0) {
                syncCandidates.assign(true);
                oldPacket=packet;
            } else if(count == 1) {
                // find offset
                for(int ii=0; ii<PACKETSIZE; ++ii)
                    if(syncCandidates[ii]) {
                        syncOffset=ii;
                        break;
                    }
                    // set synced
                    syncCandidates.assign(false);
                    syncCandidates[0]=true;
                    // shift data in oldPacket and packet to put the frame bytes into slot 0
                    for(int ii=0; ii<PACKETSIZE-syncOffset; ++ii) 
                        oldPacket[ii]=oldPacket[syncOffset+ii]; 
                    for(int ii=PACKETSIZE-syncOffset; ii<PACKETSIZE; ++ii) 
                        oldPacket[ii]=packet[ii-(PACKETSIZE-syncOffset)];
                    for(int ii=0; ii<PACKETSIZE-syncOffset; ++ii) 
                        packet[ii]=packet[syncOffset+ii];    
                    syncOffset=PACKETSIZE-syncOffset;
                    // no oldPacket=packet ! 
                    // we've shifted data inside packet to put the frame byte at the front
                    // and want to read up all the data before the next frame byte into packet
            } else {
                oldPacket=packet;
            }
        }

        listener(*this);
    }

    template<typename AsyncReadStream>
    void read(AsyncReadStream& stream) {    
        int realSyncOffset = syncOffset == -1 ? 0 : syncOffset;
        boost::asio::async_read(stream, 
            boost::asio::buffer(packet.c_array()+realSyncOffset, PACKETSIZE-realSyncOffset),
            boost::bind<void>(boost::ref(*this), _1, _2));
    }
};

// warning 4355 : 'this' used in initializer list
#pragma warning(disable:4355)
struct Device : public device::Device {
    // io
    boost::asio::io_service& io_service;
    std::string port;
    shared_ptr<boost::asio::serial_port> serial_portP;
    Reader<Device> reader;
    // sample output
    boost::array<Property<uint8_t>,2> alcval;
    boost::array<double,2> sampledata;
    Sample samplestruct;

    Device(boost::asio::io_service& io_service_, const char* port_) :
            device::Device(2,2), // 2 buttons, 2 channels
            io_service(io_service_),
            port(port_),
            serial_portP(),
            reader(*this), 
            samplestruct(sampledata.c_array(), sampledata.size()) {
        reset();
        read();
    }
    void read() {
        reader.read(*serial_portP);
    }
    void reset() {
        shared_ptr<boost::asio::serial_port> 
            new_serial_portP(new boost::asio::serial_port(io_service));
        serial_portP=new_serial_portP; // old port should be destroyed here
        serial_portP->open(port);      // so we should be able to open                                     
        using boost::asio::serial_port_base;
        serial_portP->set_option(serial_port_base::baud_rate(BAUDRATE));
        serial_portP->set_option(serial_port_base::character_size(8));
        serial_portP->set_option(serial_port_base::parity(serial_port_base::parity::none));
        serial_portP->set_option(serial_port_base::stop_bits(serial_port_base::stop_bits::one));
        serial_portP->set_option(serial_port_base::flow_control(serial_port_base::flow_control::none));
    }

    void updateStatus(uint8_t framingbyte) {
        uint8_t type = framingbyte>>6;
        if(type==0) {
            battery.set(!((framingbyte >> 5) & 1));
            const int possible_sampling_rates[] = { 122, 128, 256, 512 };
            sampling_rate.set(possible_sampling_rates[framingbyte & 3]);
        } else if(type==2) {
            button[0]->set((framingbyte >> 4) & 1);
            button[1]->set((framingbyte >> 5) & 1);
            channel_ok[0]->set((framingbyte >> 0) & 1);
            channel_ok[1]->set((framingbyte >> 1) & 1);
        } else {
            alcval[type==1 ? 0 : 1].set(framingbyte & (~(3 << 6)));
        }
    }
    
    template<typename Reader>
    void operator()(Reader& reader) {
        if(reader.synchronized()) { 
            sync.set(true);
            updateStatus(reader.packet[0]);
            if(alcval[0].initialized && alcval[1].initialized) {
                for(int ii=0; ii<2; ++ii) {
                    uint16_t usample = *reinterpret_cast<uint16_t*>(&reader.packet[2*ii+1]);
                    // interpret left 12 bits as a 12-bit signed integer
                    std::bitset<16> mask;
                    mask.set(11);
                    int16_t  ssample=int16_t((usample>>4)&uint16_t((~mask).to_ulong())) - // chop off the sign bit 
                                     int16_t((usample>>4)&uint16_t(mask.to_ulong()));     // sign*maxabs12bitval 
                    // FIXME: the first couple samples come out 0 or 1.75e272 but the others are right
                    sampledata[ii]=double(ssample)*VOLTAGEMULTIPLIER*(ABOVEALC-alcval[ii]());
                }
            }
            sample(samplestruct);
        } else {
            sync.set(false);
        }

        io_service.post(boost::bind(&Device::read, this));
    }
};

}; //namespace pendant
}; //namespace device

#endif
